import numpy as np

# 双目相机参数（通过Matlab标定工具箱获得）
class stereoCamera(object):
    def __init__(self):
        # 左相机内参
        self.cam_matrix_left = np.array([[317.4218635, 0.051598611, 310.3918248],
                                         [0., 317.4800475, 246.5242751],
                                         [0., 0., 1.]])
        # 右相机内参
        self.cam_matrix_right = np.array([[317.2139274, 0.016785116, 294.111047],
                                          [0., 317.3391035, 233.636449],
                                          [0., 0., 1.]])
        # 左右相机畸变系数:[k1, k2, p1, p2, k3]
        self.distortion_l = np.array([[0.029923888, -0.198727473, 0.000850878, 0.000820745, 0.099983063]])
        self.distortion_r = np.array([[0.010818007, -0.186973253, 0.002295726, -0.002781668, 0.099966298]])
        # 旋转矩阵
        self.R = np.array([[0.997761961, -0.052790715, -0.041039114],
                           [0.053649884, 0.998357027, 0.020123052],
                           [0.039909378, -0.02227976, 0.998954881]])
        # 平移矩阵
        self.T = np.array([[-4.798423535], [-1.205302087], [0.1433059]])
        # 焦距
        #self.focal_length = 1602.46406  # 默认值，一般取立体校正后的重投影矩阵Q中的 Q[2,3]
        # 基线距离
        self.baseline = 4.798423535  # 单位：mm， 为平移向量的第一个参数的绝对值